#include "Algorithm.h"

namespace StereoCorrespond {
	
#define CLAMP0_255(val) Math::Max(Math::Min(val, 255), 0)
#define CLAMP0_255f(val) Math::Max(Math::Min(val, 255.0f), 0.0f)

	Bitmap^ toCLRBitmap(Algorithm::Bitmap<int>* bitmap) {
		Bitmap^ img = gcnew Bitmap(bitmap->getWidth(), bitmap->getHeight());
		for(unsigned i=0; i<bitmap->getWidth(); ++i) {
			for(unsigned j=0; j<bitmap->getHeight(); ++j) {
				int p = CLAMP0_255(bitmap->getPixel(i, j));
				img->SetPixel(i, j, Color::FromArgb(p, p, p));
			}
		}
		return img;
	}

	Bitmap^ toCLRBitmap(Algorithm::Bitmap<int2>* bitmap) {
		Bitmap^ img = gcnew Bitmap(bitmap->getWidth(), bitmap->getHeight());
		for(unsigned i=0; i<bitmap->getWidth(); ++i) {
			for(unsigned j=0; j<bitmap->getHeight(); ++j) {
				int2 p = bitmap->getPixel(i, j);
				img->SetPixel(i, j, Color::FromArgb(CLAMP0_255(p.x), CLAMP0_255(p.y), 0));
			}
		}
		return img;
	}

	Bitmap^ toCLRBitmap(Algorithm::Bitmap<int3>* bitmap) {
		Bitmap^ img = gcnew Bitmap(bitmap->getWidth(), bitmap->getHeight());
		for(unsigned i=0; i<bitmap->getWidth(); ++i) {
			for(unsigned j=0; j<bitmap->getHeight(); ++j) {
				int3 p = bitmap->getPixel(i, j);
				img->SetPixel(i, j, Color::FromArgb(CLAMP0_255(p.x), CLAMP0_255(p.y), CLAMP0_255(p.z)));
			}
		}
		return img;
	}

	Bitmap^ toCLRBitmap(Algorithm::Bitmap<int4>* bitmap) {
		Bitmap^ img = gcnew Bitmap(bitmap->getWidth(), bitmap->getHeight());
		for(unsigned i=0; i<bitmap->getWidth(); ++i) {
			for(unsigned j=0; j<bitmap->getHeight(); ++j) {
				int4 p = bitmap->getPixel(i, j);
				img->SetPixel(i, j, Color::FromArgb(CLAMP0_255(p.w), CLAMP0_255(p.x), CLAMP0_255(p.y), CLAMP0_255(p.z)));
			}
		}
		return img;
	}
	
	Algorithm::Bitmap<int>* toBitmap_int(Bitmap^ bitmap) {
		Algorithm::Bitmap<int>* img = new Algorithm::Bitmap<int>(bitmap->Width, bitmap->Height);
		for(int i=0; i<bitmap->Width; ++i) {
			for(int j=0; j<bitmap->Height; ++j) {
				Color^ col = bitmap->GetPixel(i, j);
				int& destc = img->getPixel(i, j);
				destc = col->R;
			}
		}
		return img;
	}

	Algorithm::Bitmap<int2>* toBitmap_int2(Bitmap^ bitmap) {
		Algorithm::Bitmap<int2>* img = new Algorithm::Bitmap<int2>(bitmap->Width, bitmap->Height);
		for(int i=0; i<bitmap->Width; ++i) {
			for(int j=0; j<bitmap->Height; ++j) {
				Color^ c = bitmap->GetPixel(i, j);
				int2& destc = img->getPixel(i, j);
				destc.x = c->R;
				destc.y = c->G;
			}
		}
		return img;
	}

	Algorithm::Bitmap<int3>* toBitmap_int3(Bitmap^ bitmap) {
		Algorithm::Bitmap<int3>* img = new Algorithm::Bitmap<int3>(bitmap->Width, bitmap->Height);
		for(int i=0; i<bitmap->Width; ++i) {
			for(int j=0; j<bitmap->Height; ++j) {
				Color^ c = bitmap->GetPixel(i, j);
				int3& destc = img->getPixel(i, j);
				destc.x = c->R;
				destc.y = c->G;
				destc.z = c->B;
			}
		}
		return img;
	}

	Algorithm::Bitmap<int4>* toBitmap_int4(Bitmap^ bitmap) {
		Algorithm::Bitmap<int4>* img = new Algorithm::Bitmap<int4>(bitmap->Width, bitmap->Height);
		for(int i=0; i<bitmap->Width; ++i) {
			for(int j=0; j<bitmap->Height; ++j) {
				Color^ c = bitmap->GetPixel(i, j);
				int4& destc = img->getPixel(i, j);
				destc.x = c->R;
				destc.y = c->G;
				destc.z = c->B;
				destc.w = c->A;
			}
		}
		return img;
	}

	Bitmap^ toCLRBitmap(Algorithm::Bitmap<float>* bitmap) {
		Bitmap^ img = gcnew Bitmap(bitmap->getWidth(), bitmap->getHeight());
		for(unsigned i=0; i<bitmap->getWidth(); ++i) {
			for(unsigned j=0; j<bitmap->getHeight(); ++j) {
				float p = CLAMP0_255f(bitmap->getPixel(i, j));
				img->SetPixel(i, j, Color::FromArgb((int)p, (int)p, (int)p));
			}
		}
		return img;
	}

	Bitmap^ toCLRBitmap(Algorithm::Bitmap<float2>* bitmap) {
		Bitmap^ img = gcnew Bitmap(bitmap->getWidth(), bitmap->getHeight());
		for(unsigned i=0; i<bitmap->getWidth(); ++i) {
			for(unsigned j=0; j<bitmap->getHeight(); ++j) {
				float2 p = bitmap->getPixel(i, j);
				img->SetPixel(i, j, Color::FromArgb((int)CLAMP0_255f(p.x), (int)CLAMP0_255f(p.y), 0));
			}
		}
		return img;
	}

	Bitmap^ toCLRBitmap(Algorithm::Bitmap<float3>* bitmap) {
		Bitmap^ img = gcnew Bitmap(bitmap->getWidth(), bitmap->getHeight());
		for(unsigned i=0; i<bitmap->getWidth(); ++i) {
			for(unsigned j=0; j<bitmap->getHeight(); ++j) {
				float3 p = bitmap->getPixel(i, j);
				img->SetPixel(i, j, Color::FromArgb((int)CLAMP0_255f(p.x), (int)CLAMP0_255f(p.y), (int)CLAMP0_255f(p.z)));
			}
		}
		return img;
	}

	Bitmap^ toCLRBitmap(Algorithm::Bitmap<float4>* bitmap) {
		Bitmap^ img = gcnew Bitmap(bitmap->getWidth(), bitmap->getHeight());
		for(unsigned i=0; i<bitmap->getWidth(); ++i) {
			for(unsigned j=0; j<bitmap->getHeight(); ++j) {
				float4 p = bitmap->getPixel(i, j);
				img->SetPixel(i, j, Color::FromArgb((int)CLAMP0_255f(p.w), (int)CLAMP0_255f(p.x), (int)CLAMP0_255f(p.y), (int)CLAMP0_255f(p.z)));
			}
		}
		return img;
	}
	
	Algorithm::Bitmap<float>* toBitmap_float(Bitmap^ bitmap) {
		Algorithm::Bitmap<float>* img = new Algorithm::Bitmap<float>(bitmap->Width, bitmap->Height);
		for(int i=0; i<bitmap->Width; ++i) {
			for(int j=0; j<bitmap->Height; ++j) {
				Color^ col = bitmap->GetPixel(i, j);
				float& destc = img->getPixel(i, j);
				destc = col->R;
			}
		}
		return img;
	}

	Algorithm::Bitmap<float2>* toBitmap_float2(Bitmap^ bitmap) {
		Algorithm::Bitmap<float2>* img = new Algorithm::Bitmap<float2>(bitmap->Width, bitmap->Height);
		for(int i=0; i<bitmap->Width; ++i) {
			for(int j=0; j<bitmap->Height; ++j) {
				Color^ c = bitmap->GetPixel(i, j);
				float2& destc = img->getPixel(i, j);
				destc.x = c->R;
				destc.y = c->G;
			}
		}
		return img;
	}

	Algorithm::Bitmap<float3>* toBitmap_float3(Bitmap^ bitmap) {
		Algorithm::Bitmap<float3>* img = new Algorithm::Bitmap<float3>(bitmap->Width, bitmap->Height);
		for(int i=0; i<bitmap->Width; ++i) {
			for(int j=0; j<bitmap->Height; ++j) {
				Color^ c = bitmap->GetPixel(i, j);
				float3& destc = img->getPixel(i, j);
				destc.x = c->R;
				destc.y = c->G;
				destc.z = c->B;
			}
		}
		return img;
	}

	Algorithm::Bitmap<float4>* toBitmap_float4(Bitmap^ bitmap) {
		Algorithm::Bitmap<float4>* img = new Algorithm::Bitmap<float4>(bitmap->Width, bitmap->Height);
		for(int i=0; i<bitmap->Width; ++i) {
			for(int j=0; j<bitmap->Height; ++j) {
				Color^ c = bitmap->GetPixel(i, j);
				float4& destc = img->getPixel(i, j);
				destc.x = c->R;
				destc.y = c->G;
				destc.z = c->B;
				destc.w = c->A;
			}
		}
		return img;
	}

	/*Image^ Harris(Bitmap^ src, double gaussSigma) {
		// gauss
		Filter^ gaussFilter = Filter::createGauss(gaussSigma);
		Bitmap^ gaussImage = gaussFilter->Run(src);

		// derivated
		Bitmap^ derivateImage = gcnew Bitmap(gaussImage->Width, gaussImage->Height);
		int max_width = gaussImage->Width-1;
		int max_height = gaussImage->Height-1;
		for(int i=0; i<src->Width; ++i) {
			for(int j=0; j<src->Height; ++j) {
				// clamp
				int top = Math::Max(Math::Min(max_height, j-1), 0);
				int bottom = Math::Max(Math::Min(max_height, j+1), 0);
				int left = Math::Max(Math::Min(max_width, i-1), 0);
				int right = Math::Max(Math::Min(max_width, i+1), 0);
				
				double dX = (gaussImage->GetPixel(right, j).R/255.0 - gaussImage->GetPixel(left, j).R/255.0)/2.0;
				double dY = (gaussImage->GetPixel(i, bottom).R/255.0 - gaussImage->GetPixel(i, top).R/255.0)/2.0;

				derivateImage->SetPixel(i, j, Color::FromArgb((int)((dX+1)/2*255), 0*(int)((dY+1)/2*255), 0));
			}
		}

		return derivateImage;
	}*/
}